﻿#include "ut_algorithm.h"
#include "ut_math.h"
#include <vector>
#include <list>
#include <string.h>

BEGIN_SPACE_LANGE_VDR

/**
 * @brief 计算两个角度的差值
 * @param angle1   角度1（单位：弧度）
 * @param angle2   角度2（单位：弧度）
 * @return 返回角度差
 */
float UTAlgorithm::AngleDiff(const float angle1, const float angle2)
{
	float diff = UTMath::Abs(angle1 - angle2);
	if (diff - static_cast<float>(ALG_PI) >= static_cast<float>(ALG_FLOAT_EPS))
	{
		diff = static_cast<float>(2.0 * ALG_PI) - diff;
	}
	return diff;
}

/**
 * @brief 计算两个角度的差值
 * @param angle1   角度1（单位：弧度）
 * @param angle2   角度2（单位：弧度）
 * @return 返回角度差
 */
double UTAlgorithm::AngleDiff(const double angle1, const double angle2)
{
	double diff = UTMath::Abs(angle1 - angle2);
	if (diff - ALG_PI >= ALG_DOUBLE_EPS)
	{
		diff = 2.0 * ALG_PI - diff;
	}
	return diff;
}

/**
 * @brief 点与直线位置关系
 * @param point         点
 * @param lineStart     直线起点
 * @param lineEnd       直线终点
 * @return >0           点在直线外部
 *         <0           点在直线内部
 *         0            点在直线上
 */
int32_t UTAlgorithm::PointLineStation(const MI2DPOINT &point, const MI2DPOINT &lineStart, const MI2DPOINT &lineEnd)
{
	return (lineEnd.GetX() - lineStart.GetX()) * (point.GetY() - lineStart.GetY()) -
		   (point.GetX() - lineStart.GetX()) * (lineEnd.GetY() - lineStart.GetY());
}

/**
 * @brief 点与直线位置关系
 * @param point         点
 * @param lineStart     直线起点
 * @param lineEnd       直线终点
 * @return >0           点在直线外部
 *         <0           点在直线内部
 *         0            点在直线上
 */
int32_t UTAlgorithm::PointLineStation(const MD2DPOINT &point, const MD2DPOINT &lineStart, const MD2DPOINT &lineEnd)
{
	return (int32_t)((lineEnd.GetX() - lineStart.GetX()) * (point.GetY() - lineStart.GetY()) -
					 (point.GetX() - lineStart.GetX()) * (lineEnd.GetY() - lineStart.GetY()));
}

/**
 * @brief 获取线A、线B的交点
 * @param clzLineAStart     线A起点
 * @param clzLineAEnd       线A终点
 * @param clzLineBStart     线B起点
 * @param clzLineBEnd       线B终点
 * @param clzIntersection   返回交点
 * @return true             函数执行成功
 * @return false            函数执行失败
 */
bool UTAlgorithm::GetIntersectionOfLines(const MD2DPOINT &clzLineAStart, const MD2DPOINT &clzLineAEnd,
										 const MD2DPOINT &clzLineBStart, const MD2DPOINT &clzLineBEnd,
										 MD2DPOINT &clzIntersection)
{
	double dx1 = clzLineAStart.GetX() - clzLineAEnd.GetX();
	double dy1 = clzLineAStart.GetY() - clzLineAEnd.GetY();

	double dx2 = clzLineBStart.GetX() - clzLineBEnd.GetX();
	double dy2 = clzLineBStart.GetY() - clzLineBEnd.GetY();

	if (abs(dx1 * dy2 - dx2 * dy1) < 1.0E-6)
	{
		clzIntersection.SetX((clzLineAEnd.GetX() + clzLineBStart.GetX()) / 2.0);
		clzIntersection.SetY((clzLineAEnd.GetY() + clzLineBStart.GetY()) / 2.0);
		return true;
	}

	double da = dx1 * clzLineAStart.GetY() - dy1 * clzLineAStart.GetX();
	double db = dx2 * clzLineBStart.GetY() - dy2 * clzLineBStart.GetX();

	double dx = (db * dx1 - da * dx2) / (dy1 * dx2 - dy2 * dx1);
	double dy = (da * dy2 - db * dy1) / (dx1 * dy2 - dx2 * dy1);

	clzIntersection.SetX(dx);
	clzIntersection.SetY(dy);

	return true;
}

/**
 * @brief 用四边形裁剪直线
 * @param clzLeftTop        四边形 left、top 顶点
 * @param clzRightTop       四边形 right、top 顶点
 * @param clzRightBottom    四边形 right、bottom 顶点
 * @param clzLeftBottom     四边形 left、bottom 顶点
 * @param[in out] clzLineStart      直线起点
 * @param[in out] clzLineEnd        直线终点
 * @return >0               直线已被成功裁剪，通过 clzLineStart、clzLineEnd 返回
 * @return 0                直线在矩形外应该丢弃
 * @return <0               剪裁失败，函数执行错误
 */
int32_t UTAlgorithm::ClipLineWidthQuad(const MD2DPOINT &clzLeftTop, const MD2DPOINT &clzRightTop,
									   const MD2DPOINT &clzRightBottom, const MD2DPOINT &clzLeftBottom,
									   MD2DPOINT &clzLineStart, MD2DPOINT &clzLineEnd)
{
	MD2DPOINT aQuad[4] = {clzLeftTop, clzRightTop, clzRightBottom, clzLeftBottom};

	for (int32_t i = 0; i != 4; ++i)
	{
		// 四边形边
		const int32_t iQuadBorderStart = i;
		const int32_t iQuadBorderEnd = (i + 1) % 4;

		// 计算起点、终点与四边形边的位置关系
		const int32_t iLineStartPos = PointLineStation(clzLineStart, aQuad[iQuadBorderStart], aQuad[iQuadBorderEnd]);
		const int32_t iLineEndPos = PointLineStation(clzLineEnd, aQuad[iQuadBorderStart], aQuad[iQuadBorderEnd]);

		// 线在边界外侧
		if (iLineStartPos >= 0 && iLineEndPos >= 0)
		{
			if (0 == iLineStartPos && 0 == iLineEndPos)
			{
				return 1;
			}
			else
			{
				return 0; // 现在四边形外应该被丢弃
			}
		}
		// 线与边界相交
		else if (iLineStartPos >= 0 && iLineEndPos < 0)
		{ // 起点在直线外、终点在直线内
			if (false == GetIntersectionOfLines(aQuad[iQuadBorderStart], aQuad[iQuadBorderEnd], clzLineStart, clzLineEnd, clzLineStart))
			{
				return -1;
			}
		}
		else if (iLineStartPos < 0 && iLineEndPos >= 0)
		{ // 起点在直线内，终点在直线外
			if (false == GetIntersectionOfLines(aQuad[iQuadBorderStart], aQuad[iQuadBorderEnd], clzLineStart, clzLineEnd, clzLineEnd))
			{
				return -2;
			}
		}
		// 直线在矩形内
		else
		{
		}
	}

	return 1;
}

/**
 * @brief 点与三角形位置关系
 * @param point        点
 * @param triOne       三角形第一个点
 * @param triTwo       三角形第二个点
 * @param triThree     三角形第三个点
 * @return >0          点在三角形外
 *         <0          点在三角形内
 *         ==0         在三角形上
 */
int32_t UTAlgorithm::PointTriangleStation(const MI2DPOINT &point,
										  const MI2DPOINT &triOne, const MI2DPOINT &triTwo, const MI2DPOINT &triThree)
{
	const int32_t a = PointLineStation(point, triOne, triTwo);
	const int32_t b = PointLineStation(point, triTwo, triThree);
	const int32_t c = PointLineStation(point, triThree, triOne);
	if ((a > 0 && b > 0 && c > 0) || (a < 0 && b < 0 && c < 0)) // point在三角形内
	{
		return -1;
	}
	if (a == 0 || b == 0 || c == 0) // point在三角形上
	{
		return 0;
	}
	return 1; // point在三角形外
}

/**
 * @brief 点与矩形位置关系
 * @param pt        点
 * @param rect      矩形
 * @return true     点在矩形内
 * @return false    点在矩形外
 */
bool UTAlgorithm::PointRectangleStation(const MI2DPOINT &pt, const MIUTRECT &rect)
{
	const int32_t left = rect.GetMinX();
	const int32_t top = rect.GetMinY();
	const int32_t right = rect.GetMaxX();
	const int32_t bottom = rect.GetMaxY();
	if (((pt.GetX() >= left && pt.GetX() <= right) || (pt.GetX() >= right && pt.GetX() <= left)) &&
		((pt.GetY() >= top && pt.GetY() <= bottom) || (pt.GetY() >= bottom && pt.GetY() <= top)))
	{
		return true;
	}
	return false;
}

/**
 * @brief 点与多边形位置关系
 * @param point       点
 * @param vertexes    多边形顶点数组
 * @param count       多边形顶点个数
 * @return true       点在多边形内
 * @return false      点在多边形外
 */
bool UTAlgorithm::PointInPolygon(const MI2DPOINT &point, const MI2DPOINT *vertexes, const int32_t count)
{
	if (nullptr == vertexes || count <= 0)
	{
		return false;
	}
	int32_t nCross = 0;
	for (int32_t i = 0; i < count; i++)
	{
		MI2DPOINT p1 = vertexes[i];
		MI2DPOINT p2 = vertexes[(i + 1) % count];
		// 求解y=p.y与p1p2的交点
		if (p1.GetY() == p2.GetY()) // p1p2与y=p0.y平行
		{
			continue;
		}
		if (point.GetY() < UTMath::Min(p1.GetY(), p2.GetY())) //交点在p1p2延长线上
		{
			continue;
		}
		if (point.GetY() >= UTMath::Max(p1.GetY(), p2.GetY())) //交点在p1p2延长线上
		{
			continue;
		}
		// 求交点的 X 坐标
		double x = (double)(point.GetY() - p1.GetY()) * (double)(p2.GetX() - p1.GetX()) /
					   (double)(p2.GetY() - p1.GetY()) +
				   p1.GetX();
		if (x > point.GetX())
		{
			nCross++; // 只统计单边交点
		}
	}
	// 单边交点为偶数,点在多边形之外
	return nCross % 2 == 1;
}

/**
 * @brief 计算点到直线距离的平方，并求出垂足
 * @param point             点       (真实经纬度乘以3600000.0)
 * @param lineStart         直线起点  (真实经纬度乘以3600000.0)
 * @param lineEnd           直线终点  (真实经纬度乘以3600000.0)
 * @param rate              误差率    (没有误差就填1)
 * @param verticalPoint     垂足点    (真实经纬度乘以3600000.0)
 * @return 返回颠倒直线距离的平方，通过参数返回垂足
 */
float UTAlgorithm::Point2LineDistance(const MI2DPOINT &point, const MI2DPOINT &lineStart,
									  const MI2DPOINT &lineEnd, const float rate, MI2DPOINT &verticalPoint)
{
	double dX = lineEnd.GetX() - lineStart.GetX();
	double dY = lineEnd.GetY() - lineStart.GetY();

	double dR = -(lineStart.GetY() - point.GetY()) * dY - (lineStart.GetX() - point.GetX()) * dX * rate;
	double dL = dX * dX * rate + dY * dY;

	if (lineStart.GetX() == lineEnd.GetX()) //直线平行于Y轴
	{
		verticalPoint.SetX(lineStart.GetX());
		verticalPoint.SetY(point.GetY());
	}
	else if (lineStart.GetY() == lineEnd.GetY()) //直线平行于X轴
	{
		verticalPoint.SetX(point.GetX());
		verticalPoint.SetY(lineStart.GetY());
	}
	else // 既不平行于X轴又不平行于Y轴
	{
		verticalPoint.SetX(lineStart.GetX() + (int32_t)(dR * dX / dL));
		verticalPoint.SetY(lineStart.GetY() + (int32_t)(dR * dY / dL));
	}

	dX = point.GetX() - verticalPoint.GetX();
	dY = point.GetY() - verticalPoint.GetY();

	return (float)(dX * dX * rate + dY * dY);
}

/**
 * @brief 计算点到直线垂直距离的平方，并求出垂足
 * @param Point               点       屏幕象素坐标
 * @param VerticalPoint       垂足点   屏幕象素坐标
 * @param lineStart           直线起点 屏幕象素坐标
 * @param lineEnd             直线终点 屏幕象素坐标
 * @param rate                误差率(没有误差就填为1)
 * @return 点到直线垂直距离的平方和垂足点
 */
float UTAlgorithm::Point2LineDis(const MF2DPOINT &Point, MF2DPOINT &VerticalPoint,
								 const MF2DPOINT &lineStart, const MF2DPOINT &lineEnd, const float rate)
{
	double dX = lineEnd.GetX() - lineStart.GetX();
	double dY = lineEnd.GetY() - lineStart.GetY();

	double dR = -(lineStart.GetY() - Point.GetY()) * dY - (lineStart.GetX() - Point.GetX()) * dX * rate;
	double dL = dX * dX * rate + dY * dY;

	if (0 == UTMath::Compare(lineStart.GetX(), lineEnd.GetX())) // 直线平行于Y轴
	{
		VerticalPoint.SetX(lineStart.GetX());
		VerticalPoint.SetY(Point.GetY());
	}
	else if (0 == UTMath::Compare(lineStart.GetY(), lineEnd.GetY())) // 直线平行于X轴
	{
		VerticalPoint.SetX(Point.GetX());
		VerticalPoint.SetY(lineStart.GetY());
	}
	else // 既不平行于X轴又不平行于Y轴
	{
		VerticalPoint.SetX(lineStart.GetX() + (int32_t)(dR * dX / dL));
		VerticalPoint.SetY(lineStart.GetY() + (int32_t)(dR * dY / dL));
	}

	dX = Point.GetX() - VerticalPoint.GetX();
	dY = Point.GetY() - VerticalPoint.GetY();

	return (float)(dX * dX * rate + dY * dY);
}

/**
 * @brief 求点到曲线垂直距离的平方,并求出垂足
 * @param Point               点(绝对坐标)            真实经纬度乘以3600000.0
 * @param VerticalPoint       垂足点(返回值 绝对坐标)  真实经纬度乘以3600000.0
 * @param nCoorOrder          垂足在曲线第几个点后面(返回值 如果在延长线上,则返回-1)
 * @param pPotArray           曲线点集 (绝对坐标)     真实经纬度乘以3600000.0
 * @param lSizeOfPotArray     曲线点数量
 * @param rate                误差率(没有误差就填为1)
 * @return 点到曲线垂直距离的平方、垂足点、垂足在曲线第几个点后面 真实经纬度乘以3600000.0
 */
float UTAlgorithm::Point2CurveDis(const MI2DPOINT &Point, MI2DPOINT &VerticalPoint,
								  int32_t &nCoorOrder, MI2DPOINT *pPotArray, const long lSizeOfPotArray, const float rate)
{
	float fDis = 0.0f;
	float fMinDis = (float)3.0e+38;
	int32_t nMinIndex = -1;

	MI2DPOINT MinVerticalPoint;
	memset(&MinVerticalPoint, 0, sizeof(MinVerticalPoint));

	for (int32_t i = 0; i < lSizeOfPotArray - 1; i++)
	{
		MI2DPOINT pointS;
		memset(&pointS, 0, sizeof(pointS));
		MI2DPOINT pointE;
		memset(&pointE, 0, sizeof(pointE));
		MI2DPOINT tempVerticalPoint;
		memset(&tempVerticalPoint, 0, sizeof(tempVerticalPoint));

		pointS = pPotArray[i];
		pointE = pPotArray[i + 1];

		double dX = pointE.GetX() - pointS.GetX();
		double dY = pointE.GetY() - pointS.GetY();

		double dR = -(pointS.GetY() - Point.GetY()) * dY - (pointS.GetX() - Point.GetX()) * dX * rate;
		double dL = dX * dX * rate + dY * dY;

		if (pointS.GetX() == pointE.GetX()) // 直线平行于Y轴
		{
			tempVerticalPoint.SetX(pointS.GetX());
			tempVerticalPoint.SetY(Point.GetY());
		}
		else if (pointS.GetY() == pointE.GetY()) // 直线平行于X轴
		{
			tempVerticalPoint.SetX(Point.GetX());
			tempVerticalPoint.SetY(pointS.GetY());
		}
		else // 既不平行于X轴又不平行于Y轴
		{
			tempVerticalPoint.SetX(pointS.GetX() + (int32_t)(dR * dX / dL));
			tempVerticalPoint.SetY(pointS.GetY() + (int32_t)(dR * dY / dL));
		}

		if ((tempVerticalPoint.GetX() > pointS.GetX() && tempVerticalPoint.GetX() < pointE.GetX()) ||
			(tempVerticalPoint.GetX() < pointS.GetX() && tempVerticalPoint.GetX() > pointE.GetX()) ||
			(tempVerticalPoint.GetY() > pointS.GetY() && tempVerticalPoint.GetY() < pointE.GetY()) ||
			(tempVerticalPoint.GetY() < pointS.GetY() && tempVerticalPoint.GetY() > pointE.GetY()) ||
			(tempVerticalPoint == pointS) || (tempVerticalPoint == pointE)) // 垂直点在线段的起点和终点之间
		{
			fDis = CalculateSquareDistance((float)Point.GetX(), (float)Point.GetY(),
										   (float)tempVerticalPoint.GetX(), (float)tempVerticalPoint.GetY(), rate);
			if (fDis < fMinDis)
			{
				fMinDis = fDis;
				nMinIndex = i;
				MinVerticalPoint = tempVerticalPoint;
			}
		}
		else //  垂直点在线段的起点或者终点后
		{
			float fDist1 = CalculateSquareDistance((float)Point.GetX(), (float)Point.GetY(),
												   (float)pointS.GetX(), (float)pointS.GetY(), rate);
			float fDist2 = CalculateSquareDistance((float)Point.GetX(), (float)Point.GetY(),
												   (float)pointE.GetX(), (float)pointE.GetY(), rate);
			if (fDist1 < fMinDis)
			{
				fMinDis = fDist1;
				nMinIndex = i;
				MinVerticalPoint = pointS;
			}

			if (fDist2 < fMinDis)
			{
				fMinDis = fDist2;
				nMinIndex = i + 1;
				MinVerticalPoint = pointE;
			}
		}
	}

	if ((nMinIndex >= 0) && (nMinIndex < lSizeOfPotArray - 1))
	{
		nCoorOrder = nMinIndex;
		VerticalPoint = MinVerticalPoint;
	}
	else
	{
		nCoorOrder = -1;
	}

	return fMinDis;
}

/**
 * @brief 将一个点插入一条曲线中,求出它在哪个曲线点的后面
 * @param pPotArray              曲线点数组指针
 * @param lSizeOfPotArray        曲线点数量
 * @param pot                    插入的点
 * @return -1                    没有找到合适的曲线点
 * @return !=-1                  找到合适的曲线点,返回曲线点的索引
 */
int64_t UTAlgorithm::GetInsertPotPlace(const MI2DPOINT *pPotArray,
									   const int64_t lSizeOfPotArray, const MI2DPOINT &pot)
{
	for (int64_t i = 0; i < lSizeOfPotArray - 1; i++)
	{
		MI2DPOINT pos1;
		memset(&pos1, 0, sizeof(pos1));
		MI2DPOINT pos2;
		memset(&pos2, 0, sizeof(pos2));
		MI2DPOINT point;
		memset(&point, 0, sizeof(point));

		pos1 = pPotArray[i];
		pos2 = pPotArray[i + 1];

		if (pos1.GetY() == pos2.GetY()) //路段平行于X轴
		{
			point.SetX(pot.GetX());
			point.SetY(pos1.GetY());
		}
		else // 路段不平行于X轴,才能用点斜式方程式
		{
			int64_t A1 = pos1.GetY() - pos2.GetY();
			int64_t B1 = pos2.GetX() - pos1.GetX();
			int64_t C1 = pos1.GetX() * pos2.GetY() - pos1.GetY() * pos2.GetX();

			int64_t A2 = pos1.GetX() - pos2.GetX();
			int64_t B2 = pos1.GetY() - pos2.GetY();
			int64_t C2 = (pos2.GetX() - pos1.GetX()) * pot.GetX() + (pos2.GetY() - pos1.GetY()) * pot.GetY();

			point.SetX(static_cast<int32_t>((B1 * C2 - B2 * C1) / (A1 * B2 - A2 * B1)));
			point.SetY(static_cast<int32_t>((A2 * C1 - A1 * C2) / (A1 * B2 - A2 * B1)));
		}

		if (pos1.GetX() == pos2.GetX()) //路段平行于Y轴,比较Y
		{
			if ((point.GetY() >= pos1.GetY() && point.GetY() <= pos2.GetY()) ||
				(point.GetY() <= pos1.GetY() && point.GetY() >= pos2.GetY()))
			{
				return i;
			}
		}
		else // 其他比较X
		{
			if ((point.GetX() >= pos1.GetX() && point.GetX() <= pos2.GetX()) ||
				(point.GetX() <= pos1.GetX() && point.GetX() >= pos2.GetX()))
			{
				return i;
			}
		}
	}

	return -1;
}

/**
 * @brief 计算两点连线的角度
 * @param dwStartLon    点1经度 真实经纬度乘以3600000.0
 * @param dwStartLat    点1纬度 真实经纬度乘以3600000.0
 * @param dwEndLon      点2经度 真实经纬度乘以3600000.0
 * @param dwEndLat      点2纬度 真实经纬度乘以3600000.0
 * @return 与正北方向(顺时针旋转)之间的夹角(弧度)
 */
double UTAlgorithm::CalcAngle(const int32_t dwStartLon, const int32_t dwStartLat,
							  const int32_t dwEndLon, const int32_t dwEndLat)
{
	double dStartLon = (double)(dwStartLon / 3600000.0);
	double dStartLat = (double)(dwStartLat / 3600000.0);
	double dEndLon = (double)(dwEndLon / 3600000.0);
	double dEndLat = (double)(dwEndLat / 3600000.0);

	double dAngle;
	if (0 != UTMath::Compare(dEndLon, dStartLon))
	{
		double s = cos((dEndLat + dStartLat) * 0.008726646);

		double dAtan = (dEndLat - dStartLat) / ((dEndLon - dStartLon) * s);
		dAngle = atan(dAtan);
		if (dEndLon - dStartLon < 0)
		{
			dAngle += ALG_PI;
		}
		else
		{
			if (dAngle < 0)
			{
				dAngle += 2.0 * ALG_PI;
			}
		}
	}
	else
	{
		if (dEndLat > dStartLat)
		{
			dAngle = ALG_PI / 2.0;
		}
		else
		{
			dAngle = ALG_PI / 2.0 * 3.0;
		}
	}

	dAngle = ALG_PI * 5.0 / 2.0 - dAngle;
	if (dAngle > ALG_PI * 2.0)
	{
		dAngle = dAngle - ALG_PI * 2.0;
	}

	return dAngle;
}

/**
 * @brief 求点到直线的垂足
 * @param Line    直线点坐标列表  真实经纬度乘以3600000.0
 * @param PtA     点             真实经纬度乘以3600000.0
 * @param PtB     存储垂足点      真实经纬度乘以3600000.0
 * @return 最近点的类型 1 垂足点 2 起点 3 终点
 */
int32_t UTAlgorithm::Point2LineVertical(const int32_t (&Line)[4], const int32_t (&PtA)[2], int32_t (&PtB)[2])
{
	int type = 1;
	double fLine0 = (double)(Line[0] / 3600000.0);
	double fLine1 = (double)(Line[1] / 3600000.0);
	double fLine2 = (double)(Line[2] / 3600000.0);
	double fLine3 = (double)(Line[3] / 3600000.0);
	double fPtA0 = (double)(PtA[0] / 3600000.0);
	double fPtA1 = (double)(PtA[1] / 3600000.0);
	double fPtB0 = 0.0;
	double fPtB1 = 0.0;

	double dX = fLine2 - fLine0;
	double dY = fLine3 - fLine1;

	double dR = -(fLine1 - fPtA1) * dY - (fLine0 - fPtA0) * dX;

	double dL = 0.0;
	if (dR <= 0)
	{
		fPtB0 = fLine0;
		fPtB1 = fLine1;
		type = 2;
	}
	else if (dR >= (dL = dX * dX + dY * dY))
	{
		fPtB0 = fLine2;
		fPtB1 = fLine3;
		type = 3;
	}
	else
	{
		fPtB0 = fLine0 + dR * dX / dL;
		fPtB1 = fLine1 + dR * dY / dL;
	}

	PtB[0] = (int)(fPtB0 * 3600000);
	PtB[1] = (int)(fPtB1 * 3600000);
	return type;
}



/**
 * @brief 相应经度改变一度对应纬线的距离
 * @param dLati       经度
 * @return 相应经度改变一度对应纬线的距离，单位：km
 */
double UTAlgorithm::DistLatiPerDegree(const double dLati)
{
	return ALG_LENGTH_EQUATOR * sin((90.0 - dLati) * ALG_PI / 180.0) / 360.0;
}

/**
 * @brief 相应经度改变一分对应纬线的距离
 * @param dLati 经度
 * @return 相应经度改变一分对应纬线的距离，单位：km
 */
double UTAlgorithm::DistLatiPerCent(const double dLati)
{
	return DistLatiPerDegree(dLati) / 60.0;
}

/**
 * @brief 相应经度改变一秒对应纬线的距离
 * @param dLati     经度
 * @return 相应经度改变一秒对应纬线的距离，单位：km
 */
double UTAlgorithm::DistLatiPerSec(const double dLati)
{
	return DistLatiPerCent(dLati) / 60;
}

/**
 * @brief 两点之间的球面距离
 * @param x1         点1经度 DOOG数据乘以3600000.0 GPS数据乘以1000000.0
 * @param y1         点1纬度 DOOG数据乘以3600000.0 GPS数据乘以1000000.0
 * @param x2         点2经度 DOOG数据乘以3600000.0 GPS数据乘以1000000.0
 * @param y2         点2纬度 DOOG数据乘以3600000.0 GPS数据乘以1000000.0
 * @param div        真实经纬度的倍率
 *                   3600000.0  真实经纬度乘以3600000.0(DOOG二进制地图数据输出是乘以3600000.0)
 *                   1000000.0   真实经纬度乘以1000000.0(COMGPS数据输出是乘以1000000.0)
 * @return 两点之间的球面距离，单位：km
 */
double UTAlgorithm::GetMapDistance(const int32_t x1, const int32_t y1, const int32_t x2, const int32_t y2, const double div)
{
	double dx1 = (double)(x1 / div);
	double dy1 = (double)(y1 / div);
	double dx2 = (double)(x2 / div);
	double dy2 = (double)(y2 / div);

	double dLongDis = (dx1 - dx2) * 3600.0 * DistLatiPerSec(dy1 / 2.0 + dy2 / 2.0);
	double dLatiDis = (dy2 - dy1) * 3600.0 * ALG_DIST_LON_PER_SEC;

	return sqrt(dLongDis * dLongDis + dLatiDis * dLatiDis);
}

/**
 * @brief 直线差值
 * @param iStartX                      直线起点经度   DOOG数据乘以3600000.0 GPS数据乘以1000000.0
 * @param iStartY                      直线起点纬度   DOOG数据乘以3600000.0 GPS数据乘以1000000.0
 * @param iEndX                        直线终点经度   DOOG数据乘以3600000.0 GPS数据乘以1000000.0
 * @param iEndY                        直线终点纬度   DOOG数据乘以3600000.0 GPS数据乘以1000000.0
 * @param dInterpolationInterval       差值间隔（单位: km）
 * @param iOutX                        返回插值经度   DOOG数据乘以3600000.0 GPS数据乘以1000000.0
 * @param iOutY                        返回差值纬度   DOOG数据乘以3600000.0 GPS数据乘以1000000.0
 * @param dDiv                         真实经纬度的倍率
 *                                     3600000.0  真实经纬度乘以3600000.0(DOOG二进制地图数据输出是乘以3600000.0)
 *                                     1000000.0  真实经纬度乘以1000000.0(COMGPS数据输出是乘以1000000.0)
 * @return true                        函数执行成功
 * @return false                       函数执行失败
 */
bool UTAlgorithm::GetLineInterpolation(const int32_t iStartX, const int32_t iStartY, const int32_t iEndX,
									   const int32_t iEndY, const double dInterpolationInterval,
									   const double dDiv, int32_t &iOutX, int32_t &iOutY)
{
	const double dLineLen = GetMapDistance(iStartX, iStartY, iEndX, iEndY, dDiv);
	if (abs(dLineLen - 0.0) < 1.0E-6)
	{
		iOutX = iEndX;
		iOutY = iEndY;
		return true;
	}

	int32_t iDiffX = iEndX - iStartX;
	int32_t iDiffY = iEndY - iStartY;
	iOutX = iStartX + (int32_t)((double)iDiffX * dInterpolationInterval / dLineLen + 0.5);
	iOutY = iStartY + (int32_t)((double)iDiffY * dInterpolationInterval / dLineLen + 0.5);

	return true;
}

/**
 * @brief 直线差值
 * @param iStartX                      直线起点经度   DOOG数据乘以3600000.0 GPS数据乘以1000000.0
 * @param iStartY                      直线起点纬度   DOOG数据乘以3600000.0 GPS数据乘以1000000.0
 * @param iEndX                        直线终点经度   DOOG数据乘以3600000.0 GPS数据乘以1000000.0
 * @param iEndY                        直线终点纬度   DOOG数据乘以3600000.0 GPS数据乘以1000000.0
 * @param dLineLen                     线长度（单位：km）
 * @param dInterpolationInterval       差值间隔（单位: km）
 * @param iOutX                        返回插值经度   DOOG数据乘以3600000.0 GPS数据乘以1000000.0
 * @param iOutY                        返回差值纬度   DOOG数据乘以3600000.0 GPS数据乘以1000000.0
 * @param dDiv                         真实经纬度的倍率
 *                                     3600000.0  真实经纬度乘以3600000.0(DOOG二进制地图数据输出是乘以3600000.0)
 *                                     1000000.0  真实经纬度乘以1000000.0(COMGPS数据输出是乘以1000000.0)
 * @return true                        函数执行成功
 * @return false                       函数执行失败
 */
bool UTAlgorithm::GetLineInterpolation(const int32_t iStartX, const int32_t iStartY, const int32_t iEndX,
									   const int32_t iEndY, const double dLineLen, const double dInterpolationInterval,
									   const double dDiv, int32_t &iOutX, int32_t &iOutY)
{
	if (abs(dLineLen - 0.0) < 1.0E-6)
	{
		iOutX = iEndX;
		iOutY = iEndY;
		return true;
	}

	int32_t iDiffX = iEndX - iStartX;
	int32_t iDiffY = iEndY - iStartY;
	iOutX = iStartX + (int32_t)((double)iDiffX * dInterpolationInterval / dLineLen + 0.5);
	iOutY = iStartY + (int32_t)((double)iDiffY * dInterpolationInterval / dLineLen + 0.5);

	return true;
}

/**
 * @brief 计算曲线的球面长度
 * @param linePnt   曲线的坐标指针 DOOG数据乘以3600000.0 GPS数据乘以100000.0
 * @param pntcnt    曲线的坐标数量
 * @param div       真实经纬度的倍率
 *                  3600000.0  真实经纬度乘以3600000.0(DOOG二进制地图数据输出是乘以3600000.0)
 *                  100000.0   真实经纬度乘以100000.0(COMGPS数据输出是乘以100000.0)
 * @return 曲线的球面长度，单位：km
 */
double UTAlgorithm::GetCurveMapDistance(const MI2DPOINT *linePnt, const int32_t pntcnt, const double div)
{
	if (nullptr == linePnt || pntcnt < 2)
	{
		return 0;
	}
	double dDis = 0;
	for (int32_t i = 0; i < pntcnt - 1; i++)
	{
		dDis += GetMapDistance(linePnt[i].GetX(), linePnt[i].GetY(),
							   linePnt[i + 1].GetX(), linePnt[i + 1].GetY(), div);
	}
	return dDis;
}

/**
 * @brief 将数学角度转换成地理角度(与正北方向顺时针方向的夹角)
 * @param dAngle    数学角度
 * @return 地理角度(弧度)
 */
double UTAlgorithm::TransferAngleToGeo(const double dAngle)
{
	double angle = ALG_PI * 5.0 / 2.0 - dAngle;
	if (angle > ALG_PI * 2.0)
	{
		angle = angle - ALG_PI * 2.0;
	}
	return angle;
}

/**
 * @brief 线抽稀
 * @param InLine          抽稀前线         （真实经纬度乘以3600000.0）
 * @param nInLinePNum     抽稀前线的点数量
 * @param OutLine         抽稀后线         （真实经纬度乘以3600000.0）
 * @param nOutLinePNum    抽稀后线的点数量
 * @param nAbstractValue  抽稀参数 指定抽稀后线的点数量
 */
void UTAlgorithm::AbstractLine(const MI2DPOINT *InLine, const int32_t nInLinePNum,
							   MI2DPOINT *OutLine, int32_t &nOutLinePNum, const int32_t nAbstractValue)
{
	if (nAbstractValue <= 1)
	{
		return;
	}

	std::vector<MI2DPOINT> tempLine;
	for (int32_t i = 0; i < nInLinePNum; i++)
	{
		tempLine.push_back(InLine[i]);
	}

	int nCompareDist = (100 * 32) * (100 * 32); //以100米为抽稀距离
	int nTime = 0;
	while ((int32_t)tempLine.size() > nAbstractValue)
	{
		for (int i = 0; i < (int)tempLine.size(); i++)
		{
			if ((int)tempLine.size() <= nAbstractValue)
			{
				break;
			}

			if (i + 2 < (int)tempLine.size())
			{
				MI2DPOINT VerticalPoint;
				memset(&VerticalPoint, 0, sizeof(VerticalPoint));

				int32_t nSquareDis = (int32_t)Point2LineDistance(tempLine[i + 1], tempLine[i],
																 tempLine[i + 2], 1, VerticalPoint);
				if (nSquareDis < nCompareDist)
				{
					std::vector<MI2DPOINT>::iterator SeekMemberIt;
					SeekMemberIt = tempLine.begin() + i + 1;
					tempLine.erase(SeekMemberIt);
				}
			}
		}

		nTime++;
		nCompareDist *= nTime;
	}

	nOutLinePNum = (int)tempLine.size();
	for (int j = 0; j < nOutLinePNum; j++)
	{
		OutLine[j] = tempLine[j];
	}
}

/**
 * @brief 线抽稀
 * @param InLine          抽稀前线         屏幕象素坐标
 * @param nInLinePNum     抽稀前线的点数量
 * @param OutLine         抽稀后线(返回值) 屏幕象素坐标
 * @param nOutLinePNum    抽稀后线的点数量(返回值)
 * @param nAbstractValue  抽稀参数 指定抽稀后线的点数量
 */
void UTAlgorithm::AbstractLine(const MF2DPOINT *InLine, const int32_t nInLinePNum,
							   MF2DPOINT *OutLine, int32_t &nOutLinePNum, const int32_t nAbstractValue)
{
	if (nAbstractValue <= 1)
	{
		return;
	}

	std::vector<MF2DPOINT> tempLine;

	for (int32_t i = 0; i < nInLinePNum; i++)
	{
		tempLine.push_back(InLine[i]);
	}

	int nCompareDist = 500;
	int nTime = 0;

	while ((int32_t)tempLine.size() > nAbstractValue)
	{
		for (int32_t i = 0; i < (int32_t)tempLine.size(); i++)
		{
			if ((int32_t)tempLine.size() <= nAbstractValue)
			{
				break;
			}

			if (i + 2 < (int32_t)tempLine.size())
			{
				MF2DPOINT VerticalPoint;
				memset(&VerticalPoint, 0, sizeof(VerticalPoint));

				int nSquareDis = (int32_t)Point2LineDis(tempLine[i + 1], VerticalPoint, tempLine[i],
														tempLine[i + 2], 1);
				if (nSquareDis < nCompareDist)
				{
					std::vector<MF2DPOINT>::iterator SeekMemberIt;
					SeekMemberIt = tempLine.begin() + i + 1;
					tempLine.erase(SeekMemberIt);
				}
			}
		}

		nTime++;
		nCompareDist *= nTime;
	}

	nOutLinePNum = (int32_t)tempLine.size();
	for (int32_t j = 0; j < nOutLinePNum; j++)
	{
		OutLine[j] = tempLine[j];
	}
}

/**
 * @brief 将一个点绕中心点旋转,求出旋转后的点坐标
 * @param CenterPoint     中心点
 * @param Point           点
 * @param RotatePoint     旋转后的点
 * @param nAngle          旋转角度
 */
void UTAlgorithm::RotatePoint(const MI2DPOINT &CenterPoint, const MF2DPOINT Point,
							  MF2DPOINT &RotatePoint, const int32_t nAngle)
{
	float fAngle = (float)(nAngle * ALG_PI / 180.0);
	RotatePoint.SetX((float)((Point.GetX() - CenterPoint.GetX()) * cos(fAngle) + (Point.GetY() - CenterPoint.GetY()) * sin(fAngle) + CenterPoint.GetX()));
	RotatePoint.SetY((float)(-(Point.GetX() - CenterPoint.GetX()) * sin(fAngle) + (Point.GetY() - CenterPoint.GetY()) * cos(fAngle) + CenterPoint.GetY()));
}

/**
 * @brief 求两点间近似平面距离
 * @param pntS     起点
 * @param pntE     终点
 * @return 两点间距离
 */
double UTAlgorithm::ApproximateDistance(const MF2DLONLAT &pntS, const MF2DLONLAT &pntE)
{
	double dx = pntS.GetX() - pntE.GetX();
	double dy = pntS.GetY() - pntE.GetY();
	return sqrt(dx * dx + dy * dy) * ALG_RATIO_LATLON_TO_DIST;
}

/**
 * @brief 沿着直线pntStart-pntEnd的方向, 在此直线上找一个点, 到起始点(pntStart)的距离等于dDistance。
 * @param pntStart     起点
 * @param pntEnd       终点
 * @param dDistance    查找距离
 * @return 找到的点
 */
MF2DLONLAT UTAlgorithm::FindPoint(const MF2DLONLAT &pntStart, const MF2DLONLAT &pntEnd,
								  const double dDistance)
{
	if (0 == UTMath::Compare(pntStart.GetX(), pntEnd.GetX()) &&
		0 == UTMath::Compare(pntStart.GetY(), pntEnd.GetY()))
	{
		return pntStart;
	}

	if (dDistance < 0.00000001)
	{
		return pntStart;
	}

	double dTemp = ALG_RATIO_DTOL * dDistance;
	double dx = pntEnd.GetX() - pntStart.GetX();
	double dy = pntEnd.GetY() - pntStart.GetY();
	double dLen = dTemp / sqrt(dx * dx + dy * dy);

	MF2DLONLAT pntResult;
	pntResult.SetX(pntStart.GetX() + (float)(dx * dLen));
	pntResult.SetY(pntStart.GetY() + (float)(dy * dLen));

	return pntResult;
}

/**
 * @brief 获取数组中离端点一定距离的大概点数组
 * @param arLinePoints             结果点串
 * @param pPoints                  输入点串
 * @param nPntCount                输入点串个数
 * @param dDistance                查找距离
 * @param bDir                     是否按点串方向正向查找
 */
void UTAlgorithm::FindApproximatePointData(std::vector<MF2DLONLAT> &arLinePoints,
										   const MF2DLONLAT *pPoints, const int32_t nPntCount, const double dDistance, const bool bDir)
{
	arLinePoints.clear();
	if (bDir)
	{
		MF2DLONLAT pntFrom = pPoints[0];
		double dDist = 0.0;
		double dDistTemp = 0.0;
		int32_t iIndex = 1;
		MF2DLONLAT pntTemp;
		while (iIndex < nPntCount)
		{
			pntTemp = pPoints[iIndex];
			dDistTemp = ApproximateDistance(pntTemp, pntFrom);
			dDist += dDistTemp;
			if (dDist > dDistance)
			{
				pntTemp = FindPoint(pntFrom, pntTemp, dDistance - dDist + dDistTemp);
				arLinePoints.push_back(pntTemp);
				break;
			}
			arLinePoints.push_back(pntTemp);
			pntFrom = pntTemp;
			iIndex++;
		}
	}
	else
	{
		MF2DLONLAT pntFrom = pPoints[nPntCount - 1];
		double dDist = 0.0;
		double dDistTemp = 0.0;
		int iIndex = nPntCount - 2;
		MF2DLONLAT pntTemp;
		while (iIndex >= 0)
		{
			pntTemp = pPoints[iIndex];
			dDistTemp = ApproximateDistance(pntTemp, pntFrom);
			dDist += dDistTemp;
			if (dDist > dDistance)
			{
				pntTemp = FindPoint(pntFrom, pntTemp, dDistance - dDist + dDistTemp);
				arLinePoints.push_back(pntTemp);
				break;
			}
			arLinePoints.push_back(pntTemp);
			pntFrom = pntTemp;
			iIndex--;
		}
	}
}

/**
 * @brief 断点是否在复杂多边形内部
 * @param point            被判断的点
 * @param pPoints          复杂多边形点串
 * @param plPolyCounts     复杂多边形点串个数
 * @return true            点在多边形内部
 * @return false           点在复杂多边形外部
 */
bool UTAlgorithm::IsWithin(const MI2DPOINT &point, const MI2DPOINT *pPoints,
						   const int32_t plPolyCounts)
{
	int32_t maxx = 0, minx = 0, maxy = 0, miny = 0;
	if (nullptr != pPoints)
	{
		maxx = minx = pPoints[0].GetX();
		maxy = miny = pPoints[0].GetX();
		for (int32_t i = 0; i < plPolyCounts; i++)
		{
			if (maxx < pPoints[i].GetX())
				maxx = pPoints[i].GetX();
			if (minx > pPoints[i].GetX())
				minx = pPoints[i].GetX();
			if (maxy < pPoints[i].GetY())
				maxy = pPoints[i].GetY();
			if (miny > pPoints[i].GetY())
				miny = pPoints[i].GetY();
		}
	}

	//首先判断是否在面的外框范围内
	if (point.GetX() < minx || point.GetX() > maxx ||
		point.GetY() < miny || point.GetY() > maxy)
	{
		return false;
	}

	double dInter = 0;
	int32_t nCount = 0, counter = 0;
	MI2DPOINT p1, p2;
	p1.SetX(pPoints[0].GetX());
	p1.SetY(pPoints[0].GetY());

	for (int32_t i = 1; i <= plPolyCounts; i++)
	{
		p2 = pPoints[i % plPolyCounts];

		if (point.GetY() == UTMath::Min(p1.GetY(), p2.GetY()))
		{
			if (p1.GetY() == p2.GetY())
			{
				if (((point.GetX() < UTMath::Max(p1.GetX(), p2.GetX())) ||
					 point.GetX() == UTMath::Max(p1.GetX(), p2.GetX())) &&
					((point.GetX() > UTMath::Min(p1.GetX(), p2.GetX())) ||
					 point.GetX() == UTMath::Min(p1.GetX(), p2.GetX())))
				{
					return false;
				}
			}
			else
			{
				if ((point.GetX() == p1.GetX() && point.GetY() == p1.GetY()) ||
					(point.GetX() == p2.GetX() && point.GetY() == p2.GetY()))
				{
					return false;
				}
			}
		}
		else if (point.GetY() > UTMath::Min(p1.GetY(), p2.GetY()))
		{
			if ((point.GetY() < UTMath::Max(p1.GetY(), p2.GetY())) ||
				point.GetY() == UTMath::Max(p1.GetY(), p2.GetY()))
			{
				if ((point.GetX() < UTMath::Max(p1.GetX(), p2.GetX())) ||
					point.GetX() == UTMath::Max(p1.GetX(), p2.GetX()))
				{
					if (p1.GetY() != p2.GetY())
					{
						if (p1.GetX() == p2.GetX())
						{
							if (point.GetX() == p1.GetX())
							{
								return false;
							}
							counter++;
						}
						else
						{
							dInter = (point.GetY() - p1.GetY()) * (p2.GetX() - p1.GetX()) /
										 (p2.GetY() - p1.GetY()) +
									 (p1.GetX() - point.GetX());
							dInter /= fabs((double)(p2.GetX() - p1.GetX()));
							if (fabs(dInter) < ALG_DOUBLE_EPS)
							{
								return false;
							}
							if (dInter > 0.0)
							{
								counter++;
							}
						}
					}
				}
			}
		}
		p1 = p2;
	}
	if (counter % 2 == 0)
	{
		return false;
	}
	return true;
}

//{{======================多边形裁剪内部接口================================
typedef struct CLIP_POINT
{
	int32_t x;
	int32_t y;
	int32_t flag; /*点出入标志，0为原多边形顶点，1为入点，-1为出点*/
	CLIP_POINT()
	{
		flag = 0;
	}
} Clip_Point;

//多边形裁剪内部接口
static double _Fxy(const double dx, const double dy, const double px,
				   const double py, const double x1, const double y1, const double x2,
				   const double y2)
{
	return (dx * (y1 - py) - dy * (x1 - px)) * (dx * (y2 - py) - dy * (x2 - px));
}

//多边形裁剪内部接口
static bool _IsLineSegmentCross(const Clip_Point &p1, const Clip_Point &p2,
								const Clip_Point &p3, const Clip_Point &p4)
{
	double X1max, Y1max, X1min, Y1min;
	double X2max, Y2max, X2min, Y2min;
	double dx1, dy1, dx2, dy2;

	if (p1.x < p2.x)
	{
		X1max = p2.x;
		X1min = p1.x;
	}
	else
	{
		X1max = p1.x;
		X1min = p2.x;
	}

	if (p1.y < p2.y)
	{
		Y1max = p2.y;
		Y1min = p1.y;
	}
	else
	{
		Y1max = p1.y;
		Y1min = p2.y;
	}

	if (p3.x < p4.x)
	{
		X2max = p4.x;
		X2min = p3.x;
	}
	else
	{
		X2max = p3.x;
		X2min = p4.x;
	}

	if (p3.y < p4.y)
	{
		Y2max = p4.y;
		Y2min = p3.y;
	}
	else
	{
		Y2max = p3.y;
		Y2min = p4.y;
	}

	if (X1max < X2min || Y1max < Y2min || X1min > X2max || Y1min > Y2max)
		return false;
	else
	{
		dx1 = p1.x - p2.x;
		dx2 = p3.x - p4.x;
		dy1 = p1.y - p2.y;
		dy2 = p3.y - p4.y;

		if (_Fxy(dx1, dy1, p1.x, p1.y, p3.x, p3.y, p4.x, p4.y) <= 0 && _Fxy(dx2, dy2, p3.x, p3.y, p1.x, p1.y, p2.x, p2.y) <= 0)
			return true;
		else
			return false;
	}
}

//多边形裁剪内部接口
static int32_t _GetCrossPoint(const Clip_Point &p1, const Clip_Point &p2,
							  const Clip_Point &p3, const Clip_Point &p4, Clip_Point &res, const bool isClockWise)
{
	int32_t dOffsetX1 = p2.x - p1.x;
	int32_t dOffsetY1 = p2.y - p1.y;
	int32_t dOffsetX2 = p4.x - p3.x;
	int32_t dOffsetY2 = p4.y - p3.y;

	double delt = dOffsetX1 * dOffsetY2 - dOffsetX2 * dOffsetY1;

	if (delt > 0)
		res.flag = isClockWise ? -1 : 1; //入点
	else if (delt < 0)
		res.flag = isClockWise ? 1 : -1; //出点
	else if (delt == 0)
		res.flag = 0; //交点在裁剪多边形的边上

	if (delt == 0)
	{ //重合
		return 2;
	}
	double t2 = (dOffsetX1 * (p1.y - p3.y) - dOffsetY1 * (p1.x - p3.x)) / delt;
	res.x = (int32_t)(t2 * dOffsetX2) + p3.x;
	res.y = (int32_t)(t2 * dOffsetY2) + p3.y;
	if (res.x == p2.x && res.y == p2.y)
	{ //交点在边上
		return 3;
	}

	return 1; //正常相交
}

//多边形裁剪内部接口
static bool _InsertNewLine(const Clip_Point *p1, const int32_t iCount1, const Clip_Point *p2,
						   const int32_t iCount2, Clip_Point *p3, Clip_Point *p4, const bool isClockWise)
{
	int32_t i = 0, j = 0;
	int32_t m = 0, n = 0;
	int32_t count = 0;
	Clip_Point *pInOut = nullptr;
	if (iCount1 > iCount2)
	{
		pInOut = new Clip_Point[iCount1];
	}
	else
	{
		pInOut = new Clip_Point[iCount2];
	}
	if (nullptr == pInOut)
	{
		return false;
	}
	int32_t ct3 = 1;
	bool bFlag = false;
	int32_t iFlag = 0;
	for (i = 0; i < iCount1 - 1; i++)
	{
		count = 1;
		p3[ct3].x = p1[i].x;
		p3[ct3].y = p1[i].y;
		p3[ct3].flag = p3[ct3].flag;
		ct3 = ct3 + 1;
		for (j = 0; j < iCount2 - 1; j++)
		{
			if (_IsLineSegmentCross(p1[i], p1[i + 1], p2[j], p2[j + 1]))
			{
				Clip_Point pntTemp;
				int32_t iT = _GetCrossPoint(p1[i], p1[i + 1], p2[j], p2[j + 1], pntTemp, isClockWise);
				if (iT == 1)
				{
					pInOut[count].x = pntTemp.x;
					pInOut[count].y = pntTemp.y;
					if (bFlag && iFlag != pntTemp.flag)
					{
						pInOut[count].flag = 0;
					}
					else
					{
						pInOut[count].flag = pntTemp.flag;
						iFlag = pntTemp.flag;
					}
					bFlag = false;
					count++;
				}
				else if (iT == 2)
				{
					bFlag = true;
				}
				else
				{
					iFlag = pntTemp.flag;
					bFlag = false;
				}
			}
		}

		if (p1[i].x < p1[i + 1].x)
		{
			for (m = 1; m < count; m++)
			{
				for (n = m + 1; n < count; n++)
				{
					if (pInOut[m].x > pInOut[n].x)
					{
						pInOut[0].x = pInOut[m].x;
						pInOut[0].y = pInOut[m].y;
						pInOut[0].flag = pInOut[m].flag;

						pInOut[m].x = pInOut[n].x;
						pInOut[m].y = pInOut[n].y;
						pInOut[m].flag = pInOut[n].flag;

						pInOut[n].x = pInOut[0].x;
						pInOut[n].y = pInOut[0].y;
						pInOut[n].flag = pInOut[0].flag;
					}
				}
			}
		}
		else if (p1[i].x > p1[i + 1].x)
		{
			for (m = 1; m < count; m++)
			{
				for (n = m + 1; n < count; n++)
				{
					if (pInOut[m].x < pInOut[n].x)
					{
						pInOut[0].x = pInOut[m].x;
						pInOut[0].y = pInOut[m].y;
						pInOut[0].flag = pInOut[m].flag;

						pInOut[m].x = pInOut[n].x;
						pInOut[m].y = pInOut[n].y;
						pInOut[m].flag = pInOut[n].flag;

						pInOut[n].x = pInOut[0].x;
						pInOut[n].y = pInOut[0].y;
						pInOut[n].flag = pInOut[0].flag;
					}
				}
			}
		}

		for (m = 1; m < count; m++)
		{
			p3[ct3].x = pInOut[m].x;
			p3[ct3].y = pInOut[m].y;
			p3[ct3].flag = pInOut[m].flag;

			ct3++;
		}
	}

	p3[0].x = ct3;

	int32_t ct4 = 1;
	for (i = 0; i < iCount2 - 1; i++)
	{
		count = 1;
		p4[ct4].x = p2[i].x;
		p4[ct4].y = p2[i].y;
		p4[ct4].flag = p2[i].flag;
		ct4++;

		for (j = 0; j < iCount1 - 1; j++)
		{
			if (_IsLineSegmentCross(p1[j], p1[j + 1], p2[i], p2[i + 1]))
			{
				Clip_Point pntTemp;
				int iT = _GetCrossPoint(p1[j], p1[j + 1], p2[i], p2[i + 1], pntTemp, isClockWise);
				if (iT == 1)
				{
					pInOut[count].x = pntTemp.x;
					pInOut[count].y = pntTemp.y;
					if (bFlag && iFlag != pntTemp.flag)
					{
						pInOut[count].flag = 0;
					}
					else
					{
						pInOut[count].flag = pntTemp.flag;
						iFlag = pntTemp.flag;
					}
					bFlag = false;
					count++;
				}
				else if (iT == 2)
				{
					bFlag = true;
				}
				else
				{
					iFlag = pntTemp.flag;
					bFlag = false;
				}
			}
		}

		if (p2[i].x < p2[i + 1].x)
		{
			for (m = 1; m < count; m++)
			{
				for (n = m + 1; n < count; n++)
				{
					if (pInOut[m].x > pInOut[n].x)
					{
						pInOut[0].x = pInOut[m].x;
						pInOut[0].y = pInOut[m].y;
						pInOut[0].flag = pInOut[m].flag;

						pInOut[m].x = pInOut[n].x;
						pInOut[m].y = pInOut[n].y;
						pInOut[m].flag = pInOut[n].flag;

						pInOut[n].x = pInOut[0].x;
						pInOut[n].y = pInOut[0].y;
						pInOut[n].flag = pInOut[0].flag;
					}
				}
			}
		}
		else if (p2[i].x > p2[i + 1].x)
		{
			for (m = 1; m < count; m++)
			{
				for (n = m + 1; n < count; n++)
				{
					if (pInOut[m].x < pInOut[n].x)
					{
						pInOut[0].x = pInOut[m].x;
						pInOut[0].y = pInOut[m].y;
						pInOut[0].flag = pInOut[m].flag;

						pInOut[m].x = pInOut[n].x;
						pInOut[m].y = pInOut[n].y;
						pInOut[m].flag = pInOut[n].flag;

						pInOut[n].x = pInOut[0].x;
						pInOut[n].y = pInOut[0].y;
						pInOut[n].flag = pInOut[0].flag;
					}
				}
			}
		}
		else
		{ // x相等时比较y
			if (p2[i].y < p2[i + 1].y)
			{
				for (m = 1; m < count; m++)
				{
					for (n = m + 1; n < count; n++)
					{
						if (pInOut[m].y > pInOut[n].y)
						{
							pInOut[0].x = pInOut[m].x;
							pInOut[0].y = pInOut[m].y;
							pInOut[0].flag = pInOut[m].flag;

							pInOut[m].x = pInOut[n].x;
							pInOut[m].y = pInOut[n].y;
							pInOut[m].flag = pInOut[n].flag;

							pInOut[n].x = pInOut[0].x;
							pInOut[n].y = pInOut[0].y;
							pInOut[n].flag = pInOut[0].flag;
						}
					}
				}
			}
			else if (p2[i].y > p2[i + 1].y)
			{
				for (m = 1; m < count; m++)
				{
					for (n = m + 1; n < count; n++)
					{
						if (pInOut[m].y < pInOut[n].y)
						{
							pInOut[0].x = pInOut[m].x;
							pInOut[0].y = pInOut[m].y;
							pInOut[0].flag = pInOut[m].flag;

							pInOut[m].x = pInOut[n].x;
							pInOut[m].y = pInOut[n].y;
							pInOut[m].flag = pInOut[n].flag;

							pInOut[n].x = pInOut[0].x;
							pInOut[n].y = pInOut[0].y;
							pInOut[n].flag = pInOut[0].flag;
						}
					}
				}
			}
		}

		for (m = 1; m < count; m++)
		{
			if (ct4 > 0)
			{
				p4[ct4].x = pInOut[m].x;
				p4[ct4].y = pInOut[m].y;
				p4[ct4].flag = pInOut[m].flag;

				ct4++;
			}
		}

		p4[0].x = ct4;
	}

	if (pInOut != NULL)
	{
		delete[] pInOut;
		pInOut = NULL;
	}
	return true;
}

//多边形裁剪内部接口
static void _OutJd1(Clip_Point *p3, Clip_Point *p4,
					std::vector<Clip_Point> &Q, std::vector<int> &PntCnt, int &PolyCnt)
{
	int32_t i = 1;
	int32_t j = 1;
	int32_t ct = 1;
	PolyCnt = 0;
	Clip_Point inout;
command:
	if (i < p3[0].x)
	{
		if (p3[i].flag == 1)
		{
			inout.x = p3[i].x;
			inout.y = p3[i].y;
			inout.flag = p3[i].flag;

			p3[i].flag = 0;
			Q.push_back(p3[i]);
			ct++;
			i++;
			goto Q3;
		}
		else
		{
			i++;
			goto command;
		}
	}
	else
		goto End;

Q3:
	if (i == p3[0].x)
		i = 1;
	if (p3[i].flag != -1)
	{
		Q.push_back(p3[i]);
		ct++;
		i++;

		goto Q3;
	}
	else
		goto Q4;

Q4:
	for (j = 1; j < p4[0].x; j++)
	{
		if (p4[j].x == p3[i].x && p4[j].y == p3[i].y && p4[j].flag == p3[i].flag)
		{
			Q.push_back(p4[j]);
			j++;
			ct++;

			goto Q5;
		}
	}

Q5:
	if (j == p4[0].x)
		j = 1;
	if (p4[j].flag != 1)
	{
		Q.push_back(p4[j]);

		j++;
		ct++;

		goto Q5;
	}
	else
	{
		if (p4[j].x == inout.x && p4[j].y == inout.y && p4[j].flag == 1)
		{

			i = 1;
			j = 1;
			PntCnt.push_back(ct - 1);
			ct = 1;
			PolyCnt++;
			goto command;
		}
		else
			goto Q6;
	}

Q6:
	for (i = 1; i < p3[0].x; i++)
	{
		if (p3[i].x == p4[j].x && p3[i].y == p4[j].y && p3[i].flag == p4[j].flag)
		{
			p3[i].flag = 0;
			p4[j].flag = 0;
			Q.push_back(p3[i]);

			ct++;
			i++;

			goto Q3;
		}
	}
End:

{
}
}

//多边形裁剪内部接口
static void _OutJd2(Clip_Point *p3, Clip_Point *p4,
					std::vector<Clip_Point> &Q, std::vector<int> &PntCnt, int &PolyCnt)
{
	int32_t i = 1;
	int32_t j = 1;
	int32_t ct = 1;
	PolyCnt = 0;
	Clip_Point inout;
command:
	if (i < p3[0].x)
	{
		if (p3[i].flag == -1)
		{

			inout.x = p3[i].x;
			inout.y = p3[i].y;
			inout.flag = p3[i].flag;

			p3[i].flag = 0;
			Q.push_back(p3[i]);

			ct++;
			i++;
			goto Q3;
		}
		else
		{
			i++;
			goto command;
		}
	}
	else
		goto End;

Q3:
	if (i == p3[0].x)
		i = 1;
	if (p3[i].flag != 1)
	{
		Q.push_back(p3[i]);

		ct++;
		i++;

		goto Q3;
	}
	else
		goto Q4;

Q4:
	for (j = 1; j < p4[0].x; j++)
	{
		if (p4[j].x == p3[i].x && p4[j].y == p3[i].y && p4[j].flag == p3[i].flag)
		{
			Q.push_back(p4[j]);
			j++;
			ct++;

			goto Q5;
		}
	}

Q5:
	if (j == p4[0].x)
		j = 1;
	if (p4[j].flag != -1)
	{
		Q.push_back(p4[j]);

		j++;
		ct++;

		goto Q5;
	}
	else
	{
		if (p4[j].x == inout.x && p4[j].y == inout.y && p4[j].flag == -1)
		{
			i = 1;
			j = 1;
			PntCnt.push_back(ct - 1);
			ct = 1;
			PolyCnt++;
			goto command;
		}
		else
			goto Q6;
	}

Q6:
	for (i = 1; i < p3[0].x; i++)
	{
		if (p3[i].x == p4[j].x && p3[i].y == p4[j].y && p3[i].flag == p4[j].flag)
		{
			p3[i].flag = 0;
			Q.push_back(p3[i]);

			ct++;
			i++;

			goto Q3;
		}
	}
End:

{
}
}

/**
 * @brief 用多边形裁剪多边形（注意：被裁剪多边形和裁剪多边形的点串顺序必须保持一致，要么都是顺时针要么都是逆时针）
 * @param arResultPoints        裁剪后结果多边形组点串[out]
 * @param arResultPntCount      裁剪后结果多边形组内每个多边形点个数的数组[out]
 * @param iResultPolyCount      裁剪后多边形的个数[out]
 * @param pRgnPoints            被裁剪多边形点串[in]
 * @param iRgnPointCount        被裁剪多边形点串个数[in]
 * @param pClipper              裁剪多边形点串[in]
 * @param iCliperCount          裁剪多边形点串个数[in]
 * @param cType                 裁剪的方式（1：求交集；2：求差集；3：求并集）[in]
 * @param isClockWise           裁剪多边形和被裁剪多边形点串的排列顺序（true：顺时针；false：逆时针）[in]
 * @return true                 裁剪成功
 * @return false                裁剪失败
 */
bool UTAlgorithm::ClipRegionbyRegion(std::vector<MI2DPOINT> &arResultPoints, std::vector<int32_t> &arResultPntCount,
									 int32_t &iResultPolyCount, const MI2DPOINT *pRgnPoints, const int32_t iRgnPointCount,
									 const MI2DPOINT *pClipper, const int32_t iCliperCount, const char cType, const bool isClockWise)
{
	//当裁剪多边形与被裁剪多边形不相交时
	bool bIntersect = IsWithin(pRgnPoints[0], pClipper, iCliperCount);
	bool bTemp = false, bOutOf = false;
	for (int32_t i = 1; i < iRgnPointCount; i++)
	{
		bTemp = IsWithin(pRgnPoints[i], pClipper, iCliperCount);
		if ((bIntersect && !bTemp) || (!bIntersect && bTemp))
		{
			bOutOf = true;
			break;
		}
	}

	if (!bOutOf)
	{ //如果不相交
		if (bIntersect)
		{
			for (int32_t j = 0; j < iRgnPointCount - 1; j++)
			{
				arResultPoints.push_back(pRgnPoints[j]);
			}
			arResultPntCount.push_back(iRgnPointCount - 1);
			iResultPolyCount = 1;
			return true;
		}
		else
		{
			bool bIntersect = IsWithin(pClipper[0], pRgnPoints, iRgnPointCount);
			bOutOf = false;
			for (int32_t i = 1; i < iCliperCount; i++)
			{
				bTemp = IsWithin(pClipper[i], pRgnPoints, iRgnPointCount);
				if ((bIntersect && !bTemp) || (!bIntersect && bTemp))
				{
					bOutOf = true;
					break;
				}
			}
			if (!bOutOf)
			{
				if (bIntersect)
				{
					for (int32_t j = 0; j < iCliperCount - 1; j++)
					{
						arResultPoints.push_back(pClipper[j]);
					}
					arResultPntCount.push_back(iCliperCount - 1);
					iResultPolyCount = 1;
					return true;
				}
			}
		}
	}

	Clip_Point *pTemp1 = nullptr, *pTemp2 = nullptr, *pTemp3 = nullptr, *pTemp4 = nullptr;

	pTemp1 = new Clip_Point[iRgnPointCount];
	pTemp2 = new Clip_Point[iCliperCount];
	pTemp3 = new Clip_Point[iRgnPointCount * iCliperCount];
	pTemp4 = new Clip_Point[iRgnPointCount * iCliperCount];
	if (nullptr == pTemp1 || nullptr == pTemp2 || nullptr == pTemp3 || nullptr == pTemp4)
	{
		return false;
	}
	for (int32_t j = 0; j < iRgnPointCount; j++)
	{
		pTemp1[j].x = pRgnPoints[j].GetX();
		pTemp1[j].y = pRgnPoints[j].GetY();
		pTemp1[j].flag = 0;
	}
	for (int32_t j = 0; j < iCliperCount; j++)
	{
		pTemp2[j].x = pClipper[j].GetX();
		pTemp2[j].y = pClipper[j].GetY();
		pTemp2[j].flag = 0;
	}

	bool bInsert = _InsertNewLine(pTemp1, iRgnPointCount, pTemp2, iCliperCount,
								  pTemp3, pTemp4, isClockWise);
	if (!bInsert)
	{
		return false;
	}

	std::vector<Clip_Point> arTemp;
	if (1 == cType)
	{
		_OutJd1(pTemp3, pTemp4, arTemp, arResultPntCount, iResultPolyCount);
	}
	else if (2 == cType)
	{
		_OutJd2(pTemp3, pTemp4, arTemp, arResultPntCount, iResultPolyCount);
	}
	else if (3 == cType)
	{
		int32_t i = 0;
		int32_t iTemp = pTemp4[0].x;
		Clip_Point Temp;
		for (i = 1; i <= iTemp / 2; i++)
		{
			Temp.x = pTemp4[i].x;
			Temp.y = pTemp4[i].y;
			Temp.flag = pTemp4[i].flag;

			pTemp4[i].x = pTemp4[iTemp - i].x;
			pTemp4[i].y = pTemp4[iTemp - i].y;
			pTemp4[i].flag = pTemp4[iTemp - i].flag;

			pTemp4[iTemp - i].x = Temp.x;
			pTemp4[iTemp - i].y = Temp.y;
			pTemp4[iTemp - i].flag = Temp.flag;
		}
		_OutJd2(pTemp3, pTemp4, arTemp, arResultPntCount, iResultPolyCount);
	}

	MI2DPOINT pntTemp;
	int32_t arSize = static_cast<int32_t>(arTemp.size());
	for (int32_t j = 0; j < arSize; j++)
	{
		pntTemp.SetX(arTemp[j].x);
		pntTemp.SetY(arTemp[j].y);
		arResultPoints.push_back(pntTemp);
	}

	if (pTemp1 != NULL)
	{
		delete[] pTemp1;
		pTemp1 = NULL;
	}

	if (pTemp2 != NULL)
	{
		delete[] pTemp2;
		pTemp2 = NULL;
	}
	if (pTemp3 != NULL)
	{
		delete[] pTemp3;
		pTemp3 = NULL;
	}

	if (pTemp4 != NULL)
	{
		delete[] pTemp4;
		pTemp4 = NULL;
	}

	return true;
}

/**
 * @brief 判断多边形点串排列顺序（只能判断凸多边形）
 * @param pRgn       被多边形点串[in]
 * @param iCount     被多边形点串个数[in]
 * @return -1        顺时针
 * @return  1        逆时针
 * @return  0        在一个点上
 */
int32_t UTAlgorithm::IsClockWiseConvex(const MI2DPOINT *pRgn, const int32_t iCount)
{
	if (pRgn)
	{
		if (iCount < 3)
		{
			return 0;
		}

		int32_t i = 0, j = 0, k = 0;
		int32_t count = 0;
		double z = 0.0;

		for (i = 0; i < iCount; i++)
		{
			j = (i + 1) % iCount;
			k = (i + 2) % iCount;
			z = (pRgn[j].GetX() - pRgn[i].GetX()) * (pRgn[k].GetY() - pRgn[j].GetY());
			z -= (pRgn[j].GetY() - pRgn[i].GetY()) * (pRgn[k].GetX() - pRgn[j].GetX());
			if (z < 0)
				count--;
			else if (z > 0)
				count++;
		}
		if (count > 0)
			return (-1); // CLOCKWISE
		else if (count < 0)
			return (1); // COUNTERCLOCKWISE
		else
			return (0);
	}
	return 0;
}

/**
 * @brief 任意多边形方向的判断，包括凹凸多边形。
 * @param pRgn       被多边形点串[in]
 * @param iCount     被多边形点串个数[in]
 * @return  -1       顺时针
 * @return   1       逆时针
 * @return   0       在一个点上
 */
int32_t UTAlgorithm::IsClockWise(const MI2DPOINT *pRgn, const int32_t iCount)
{
	if (iCount < 3)
		return 0;
	double s = pRgn[0].GetY() * (pRgn[iCount - 1].GetX() - pRgn[1].GetX());
	for (int32_t i = 1; i < iCount; i++)
		s += pRgn[i].GetY() * (pRgn[(i - 1)].GetX() - pRgn[(i + 1) % iCount].GetX());
	if (s / 2 > 0)
		return -1; // CLOCKWISE
	else if (s / 2 < 0)
		return 1; // COUNTERCLOCKWISE
	else
		return 0;
}

static double PerpendicularDistance(const MI2DPOINT &Point1, const MI2DPOINT &Point2, const MI2DPOINT &Point)
{
	// Area = |(1/2)(x1y2 + x2y3 + x3y1 - x2y1 - x3y2 - x1y3)|   *Area of triangle
	// Base = v((x1-x2)2+(x1-x2)2)                               *Base of Triangle*
	// Area = .5*Base*H                                          *Solve for height
	// Height = Area/.5/Base
	const double area = 0.5 * abs(Point1.GetX() * Point2.GetY() + Point2.GetX() * Point.GetY() +
								  Point.GetX() * Point1.GetY() - Point2.GetX() * Point1.GetY() - Point.GetX() * Point2.GetY() -
								  Point1.GetX() * Point.GetY());
	const double bottom = sqrt(pow((double)Point1.GetX() - Point2.GetX(), 2) +
							   pow((double)Point1.GetY() - Point2.GetY(), 2));
	return area / bottom * 2;
}

static void _DouglasReduction(const std::vector<MI2DPOINT> &src, const int32_t firstPoint,
							  const int32_t lastPoint, const double tolerance, std::list<int32_t> &pointIndexsToKeep)
{
	if (false == src.empty())
	{
		double maxDistance = 0;
		int32_t indexFarthest = 0;

		std::vector<MI2DPOINT>::const_iterator it = src.begin();
		for (int32_t index = firstPoint; index < lastPoint; index++)
		{
			double distance = PerpendicularDistance(it[firstPoint], it[lastPoint], it[index]);
			if (distance > maxDistance)
			{
				maxDistance = distance;
				indexFarthest = index;
			}
		}

		if (maxDistance > tolerance && indexFarthest != 0)
		{
			pointIndexsToKeep.push_back(indexFarthest);
			_DouglasReduction(src, firstPoint,
							  indexFarthest, tolerance, pointIndexsToKeep);
			_DouglasReduction(src, indexFarthest,
							  lastPoint, tolerance, pointIndexsToKeep);
		}
	}
}

/**
 * @brief 线、面数据抽稀(道格拉斯-普克算法)
 * @param src          抽吸前点串[in]
 * @param dst          抽吸后点串[out]
 * @param tolerance    抽稀精度[in]
 */
void UTAlgorithm::DouglasReduction(const std::vector<MI2DPOINT> &src,
								   std::vector<MI2DPOINT> &dst, const double tolerance)
{
	if (false == src.empty())
	{
		int32_t firstPoint = 0;
		int32_t lastPoint = static_cast<int32_t>(src.size() - 1);

		//先规避 首点 末点相同
		bool bcircle = false; //是否要画环形线
		int circlelastPoint = lastPoint;
		while (src[firstPoint].GetX() == src[lastPoint].GetX() &&
			   src[firstPoint].GetY() == src[lastPoint].GetY())
		{
			bcircle = true; //如果发现相同点则设为true
			lastPoint--;
			if (lastPoint < 0)
			{
				return;
			}
		}

		std::list<int32_t> pointIndexToKeep;
		pointIndexToKeep.push_back(firstPoint);
		pointIndexToKeep.push_back(lastPoint);

		_DouglasReduction(src, firstPoint, lastPoint, tolerance, pointIndexToKeep);
		//{{add by hehongzhou 暂时解决画矩形，结果是三角形的问题
		if (bcircle)
		{
			pointIndexToKeep.push_back(circlelastPoint);
		}
		//}}
		pointIndexToKeep.sort();

		std::vector<MI2DPOINT>::const_iterator it_pnts = src.begin();
		std::list<int32_t>::iterator it_keep_point_index;
		for (it_keep_point_index = pointIndexToKeep.begin(); it_keep_point_index != pointIndexToKeep.end();
			 it_keep_point_index++)
		{
			dst.push_back(it_pnts[*it_keep_point_index]);
		}
	}
}

/**
 * @brief 计算任意多边形面积
 *         (此接口仅适用于DOOG二进制地图数据输出，即真实经纬度倍率固定为3600000.0)
 *         (请按顺时针或逆时针顺序提供多边形的各个顶点)
 * @param pPolygonPoints   多边形的顶点集合（首指针）(* 3600000)
 * @param nPointCount      多边形的顶点数目
 * @return 多边形的面积，单位：m * m ， 返回0表示计算失败
 */
double UTAlgorithm::CalcPolygonArea(const MI2DPOINT *pPolygonPoints, const int32_t nPointCount)
{
	if (nullptr == pPolygonPoints || nPointCount < 3)
	{
		return 0.0;
	}
	double dPolygonArea = 0;				  //多边形面积
	MD2DPOINT vertexA;						  //存放double坐标
	MD2DPOINT vertexB;						  //存放double坐标
	for (int32_t i = 0; i < nPointCount; i++) //根据顶点坐标，求多边形的面积
	{
		if (i < nPointCount - 1)
		{
			//将整型的点坐标转换为浮点型的经纬坐标，并参与计算面积
			vertexA.SetX((double)(pPolygonPoints[i].GetX() / ALG_COEF));
			vertexA.SetY((double)(pPolygonPoints[i].GetY() / ALG_COEF));
			vertexB.SetX((double)(pPolygonPoints[i + 1].GetX() / ALG_COEF));
			vertexB.SetY((double)(pPolygonPoints[i + 1].GetY() / ALG_COEF));
			dPolygonArea += (vertexA.GetX() * vertexB.GetY() - vertexB.GetX() * vertexA.GetY());
		}
		else
		{
			vertexA.SetX((double)(pPolygonPoints[i].GetX() / ALG_COEF));
			vertexA.SetY((double)(pPolygonPoints[i].GetY() / ALG_COEF));
			vertexB.SetX((double)(pPolygonPoints[0].GetX() / ALG_COEF));
			vertexB.SetY((double)(pPolygonPoints[0].GetY() / ALG_COEF));
			dPolygonArea += (vertexA.GetX() * vertexB.GetY() - vertexB.GetX() * vertexA.GetY());
			break;
		}
	}
	return UTMath::Abs(dPolygonArea) / 2.0;
}

void UTAlgorithm::LlaToFlat(std::vector<double>& result, double lat, double lng, double lat0, double lng0, double psi0)
{
	double lat2Radians = ToRadians(lat0);
	double sin_lat2_spuare = sin(lat2Radians);
	sin_lat2_spuare = sin_lat2_spuare * sin_lat2_spuare;
	double Rn = R / sqrt(1.0 - TEM_F * sin_lat2_spuare);
	double Rm = Rn * (1.0 - TEM_F) / (1.0 - TEM_F * sin_lat2_spuare);
	double dN = (lat - lat0) / ToDegrees(atan(1.0 / Rm));
	double dE = (lng - lng0) / ToDegrees(atan(1.0 / Rn / cos(lat2Radians)));
	result[0] = cos(psi0) * dN + sin(psi0) * dE;
	result[1] = -sin(psi0) * dN + cos(psi0) * dE;
}
void UTAlgorithm::FlatToLla(std::vector<double>& result, double flat_x, double flat_y, double lat0, double lng0, double psi0)
{
	double dN = cos(psi0) * flat_x - sin(psi0) * flat_y;
	double dE = sin(psi0) + flat_x + cos(psi0) + flat_y;
	double lat1Radians = ToRadians(lat0);
	double sin_lat1_radian = sin(lat1Radians);
	double Rn = R / sqrt(1.0 - TEM_F * sin_lat1_radian);
	double Rm = Rn * (1.0 - TEM_F) / (1.0 - TEM_F * sin_lat1_radian);
	result[0] = lat0 + dN * ToDegrees(atan(1.0 / Rm));
	result[1] = lng0 + dE * ToDegrees(atan(1.0 / Rn / cos(lat1Radians)));
}

void UTAlgorithm::CalcFootToLineSegment(std::vector<double>& result,
	double x, double y,
	double x1, double y1,
	double x2, double y2) {

	double retx, rety, distance, footDistance2start;
	double dis;
	double xDelta = x2 - x1, yDelta = y2 - y1;
	double lenSegment = xDelta * xDelta + yDelta * yDelta;

	double factor = 1.0;

	if (lenSegment < 1e-100) {
		dis = CalculateDistance(x, y, x1, y1);
		retx = x1;
		rety = y1;
		footDistance2start = 0;
	}
	else {
		double valDp = (x - x1) * xDelta + (y - y1) * yDelta;
		double  u = valDp / lenSegment;
		if (u < 0) {
			dis = CalculateDistance(x, y, x1, y1);
			retx = x1;
			rety = y1;
			footDistance2start = 0;
		}
		else if (u > 1) {
			dis = CalculateDistance(x, y, x2, y2);
			retx = x2;
			rety = y2;
			footDistance2start = CalculateDistance(x1, y1, x2, y2);
		}
		else {
			double tmpX = x1 + (valDp * xDelta) / lenSegment;
			double tmpY = y1 + (valDp * yDelta) / lenSegment;
			dis = abs((x2 - x1) * (y1 - y) - (x1 - x) * (y2 - y1)) / sqrt(lenSegment);
			retx = tmpX;
			rety = tmpY;
			footDistance2start = CalculateDistance(x1, y1, retx, rety);
		}
	}


	double footVx = x - retx;
	double footVy = y - rety;
	double lenFootSegment = footVx * footVx + footVy * footVy;

	double cos_theta = 0;

	if (lenFootSegment >1e-7) {
		cos_theta = (footVx * xDelta + footVy * yDelta) / sqrt(lenSegment) / sqrt(lenFootSegment);
	}

	result.push_back(retx);
	result.push_back(rety);
	result.push_back(dis);
	result.push_back(footDistance2start);
	result.push_back(abs(cos_theta));
}



/**
 * @brief 计算两个矩形是否相交
 * @param Src   矩形一
 * @param Dst   矩形二
 * @return true         相交
 * @return false      不相交
 */
bool UTAlgorithm::InnerVpIsRectIntersects(const MIUTRECT &Src, const MIUTRECT &Dst)
{
	if (Src.GetMinX() > Dst.GetMaxX() || Src.GetMinY() > Dst.GetMaxY() ||
		Src.GetMaxX() < Dst.GetMinX() || Src.GetMaxY() < Dst.GetMinY())
		return false;

	return true;
}

/**
 * @brief 计算线段与正北方夹角
 * @param pointS   线段起始点
 * @param pointE   线段终点
 * @return 返回夹角
 */
float UTAlgorithm::InnerVpLineAngleFromNorth(const MI2DPOINT &pointS, const MI2DPOINT &pointE)
{
	double dStartLon = (double)pointS.GetX() / 3600000.0;
	double dStartLat = (double)pointS.GetY() / 3600000.0;
	double dEndLon = (double)pointE.GetX() / 3600000.0;
	double dEndLat = (double)pointE.GetY() / 3600000.0;

	double dAngle;
	if (dEndLon != dStartLon)
	{
		double s = cos((dEndLat + dStartLat) * 0.008726646);

		double dAtan = (dEndLat - dStartLat) / ((dEndLon - dStartLon) * s);
		dAngle = atan(dAtan);
		if (dEndLon - dStartLon < 0)
		{
			dAngle += UTMath::PI;
		}
		else
		{
			if (dAngle < 0)
			{
				dAngle += 2 * UTMath::PI;
			}
		}
	}
	else
	{
		if (dEndLat > dStartLat)
		{
			dAngle = UTMath::PI / 2;
		}
		else
		{
			dAngle = UTMath::PI / 2 * 3;
		}
	}

	// 转为正北朝向夹角
	dAngle = UTMath::PI * 5 / 2 - dAngle;
	if (dAngle > UTMath::PI * 2)
		dAngle -= UTMath::PI * 2;

	return (float)dAngle;
}

void UTAlgorithm::Lla2Ecef(std::vector<double>& res, double lat, double lon, double h) {
	int a = R;
	double e_sq = f * (2 - f);
	double lamb = ToRadians(lat);
	double phi = ToRadians(lon);
	double s = sin(lamb);
	double N = a / sqrt(1 - e_sq * s * s);

	double sin_lambda = sin(lamb);
	double cos_lambda = cos(lamb);
	double sin_phi = sin(phi);
	double cos_phi = cos(phi);

	double x = (h + N) * cos_lambda * cos_phi;
	double y = (h + N) * cos_lambda * sin_phi;
	double z = (h + (1 - e_sq) * N) * sin_lambda;
	res.push_back(x);
	res.push_back(y);
	res.push_back(z);
}

void UTAlgorithm::Ecef2Enu(std::vector<double>& res, double x, double y, double z, double lat0, double lon0, double h0) {
    res.clear();
	int a = R;
	double e_sq = f * (2 - f);
	double lamb = ToRadians(lat0);
	double phi = ToRadians(lon0);
	double s = sin(lamb);
	double N = a / sqrt(1 - e_sq * s * s);

	double sin_lambda = sin(lamb);
	double cos_lambda = cos(lamb);
	double sin_phi = sin(phi);
	double cos_phi = cos(phi);

	double x0 = (h0 + N) * cos_lambda * cos_phi;
	double y0 = (h0 + N) * cos_lambda * sin_phi;
	double z0 = (h0 + (1 - e_sq) * N) * sin_lambda;

	double xd = x - x0;
	double yd = y - y0;
	double zd = z - z0;

	double t = -cos_phi * xd - sin_phi * yd;
	double xEast = -sin_phi * xd + cos_phi * yd;
	double yNorth = t * sin_lambda + cos_lambda * zd;
	double zUp = cos_lambda * cos_phi * xd + cos_lambda * sin_phi * yd + sin_lambda * zd;
	res.push_back(xEast);
	res.push_back(yNorth);
	res.push_back(zUp);
}

void  UTAlgorithm::Enu2Ecef(std::vector<double>& res, double xEast, double yNorth, double zUp, double lat0, double lon0, double h0) {
    res.clear();
	int a = R;
	double e_sq = f * (2 - f);
	double lamb = ToRadians(lat0);
	double phi = ToRadians(lon0);
	double s = sin(lamb);
	double N = a / sqrt(1 - e_sq * s * s);

	double sin_lambda = sin(lamb);
	double cos_lambda = cos(lamb);
	double sin_phi = sin(phi);
	double cos_phi = cos(phi);

	double x0 = (h0 + N) * cos_lambda * cos_phi;
	double y0 = (h0 + N) * cos_lambda * sin_phi;
	double z0 = (h0 + (1 - e_sq) * N) * sin_lambda;

	double t = cos_lambda * zUp - sin_lambda * yNorth;

	double zd = sin_lambda * zUp + cos_lambda * yNorth;
	double xd = cos_phi * t - sin_phi * xEast;
	double yd = sin_phi * t + cos_phi * xEast;
	double x = xd + x0;
	double y = yd + y0;
	double z = zd + z0;
	res.push_back(x);
	res.push_back(y);
	res.push_back(z);
}

void  UTAlgorithm::Ecef2Lla(std::vector<double>& res, double x, double y, double z) {
    res.clear();
	int a = R;
	double e_sq = f * (2 - f);
	double b = SHORT_RADIS;
	double x2 = std::pow(x, 2);
	double y2 = std::pow(y, 2);
	double z2 = std::pow(z, 2);
	double e = std::sqrt(1 - std::pow((b / a), 2));
	double b2 = b * b;
	double e2 = e * e;
	double ep = e * (a / b);
	double r = std::sqrt(x2 + y2);
	double r2 = r * r;
	double E2 = a * 1.0 * a - b * b;
	double F = 54 * b2 * z2;
	double G = r2 + (1 - e2) * z2 - e2 * E2;
	double c = (e2 * e2 * F * r2) / (G * G * G);
	double s = pow((1 + c + sqrt(c * c + 2 * c)), (1.0 / 3));
	double P = F / (3 * std::pow((s + 1 / s + 1), 2) * G * G);
	double Q = std::sqrt(1 + 2 * e2 * e2 * P);
	double ro = -(P * e2 * r) / (1 + Q) + std::sqrt((a * 1.0 * a * 1.0 / 2) * (1 + 1 / Q) - (P * (1 - e2) * z2) / (Q * (1 + Q)) - P * r2 / 2);
	double tmp = std::pow((r - e2 * ro), 2);
	double U = std::sqrt(tmp + z2);
	double V = std::sqrt(tmp + (1 - e2) * z2);
	double zo = (b2 * z) / (a * V);
	double height = U * (1 - b2 / (a * V));
	double lat = std::atan((z + ep * ep * zo) / r);
	double temp = std::atan(y / x);
	double lon = temp - ALG_PI;
	if (x >= 0) {
		lon = temp;
	}
	else if (x < 0 && y >= 0) {
		lon = ALG_PI + temp;
	}


	double lat0 = lat / (ALG_PI / 180);
	double lng0 = lon / (ALG_PI / 180);
	double h0 = height;
	res.push_back(lat0);
	res.push_back(lng0);
	res.push_back(h0);
}

void UTAlgorithm::Lla2Enu(std::vector<double>& res, double  lat, double lon, double h, double lat_ref, double lon_ref, double h_ref) {
	std::vector<double> ecef;
	Lla2Ecef(ecef, lat, lon, h);
	Ecef2Enu(res, ecef.at(0), ecef.at(1), ecef.at(2), lat_ref, lon_ref, h_ref);
}

void UTAlgorithm::Enu2Lla(std::vector<double>& res, double xEast, double yNorth, double zUp, double lat_ref, double lon_ref, double h_ref) {
	std::vector<double> ecef;
	Enu2Ecef(ecef, xEast, yNorth, zUp, lat_ref, lon_ref, h_ref);
	Ecef2Lla(res, ecef.at(0), ecef.at(1), ecef.at(2));
}

float UTAlgorithm::CalcNorm(const std::vector<float> &nums) {
    auto size = nums.size();
    if(size == 0) return 0;
    float norm = 0.0;
    for(float tmp:nums){
        norm += tmp*tmp;
    }
    return sqrt(norm);
}

    /**
        * 坐标系转换
        * @tparam T
        * @param roationMatrix
        * @param data
        * @return
        */

std::vector<float> UTAlgorithm::Rotate(std::vector<float> roationMatrix,std::vector<float> &data){
    auto size = data.size();
    std::vector<float> rotatedData(size,0);
    if(roationMatrix.size() != size * size) return rotatedData;
    for(int row = 0;row < size;row++){
        for(int col = 0; col < size ;col++){
            rotatedData[row] += roationMatrix[row * size + col] * data[col];
        }
    }
    return rotatedData;
}

template<typename T>
std::vector<T> UTAlgorithm::RotationMatrix2EulerAngle(std::vector<T> const &rotationMatrix){
    std::vector<T> eularAngle(3,0);
    if(rotationMatrix.size()!=9) return eularAngle;

    eularAngle[0] = atan2(rotationMatrix[1],rotationMatrix[4]);
    eularAngle[1] = asin(-rotationMatrix[7]);
    eularAngle[2] = atan2(-rotationMatrix[6],rotationMatrix[8]);

    return eularAngle;
}

/**
 * 四元数转旋转矩阵
 * @tparam T
 * @param quaternion 四元数(w,x,y,z)
 * @return
 */
template<typename T>
std::vector<T> UTAlgorithm::Quaternion2RotationMatrix(std::vector<T> const &quaternion){
    std::vector<T> rotationMatrix(9,0);
    if(quaternion.size()!=4) return rotationMatrix;

    const T tx = T(2)*quaternion[1];
    const T ty = T(2)*quaternion[2];
    const T tz = T(2)*quaternion[3];

    const T twx = tx * quaternion[0];
    const T twy = ty * quaternion[0];
    const T twz = tz * quaternion[0];

    const T txx = tx * quaternion[1];
    const T txy = ty * quaternion[1];
    const T txz = tz * quaternion[1];

    const T tyy = ty * quaternion[2];
    const T tyz = tz * quaternion[2];
    const T tzz = tz * quaternion[3];

    rotationMatrix[0] = T(1) -(tyy+tzz);
    rotationMatrix[1] = txy - twz;
    rotationMatrix[2] = txz + twy;
    rotationMatrix[3] = txy + twz;
    rotationMatrix[4] = T(1) - (txx+tzz);
    rotationMatrix[5] = tyz - twx;
    rotationMatrix[6] = txz - twy;
    rotationMatrix[7] = tyz + twx;
    rotationMatrix[8] = T(1) - (txx+tyy);
    return rotationMatrix;
}

template<typename T>
std::vector<T> UTAlgorithm::Quaternion2EulerAngle(std::vector<T> const &quaternion){
    std::vector<T> eulerAngle(3,0);
    if(quaternion.size()!=4) return eulerAngle;

    const T q0 = quaternion[0];
    const T q1 = quaternion[1];
    const T q2 = quaternion[2];
    const T q3 = quaternion[3];

    eulerAngle[0] = atan2(2*(q1*q2-q0*q3),1-2&(q1*q1+q3*q3));
    eulerAngle[1] = asin(-2*(q2*q3+q0*q1));
    eulerAngle[2] = atan2(-2*(q1*q3-q0*q2),1-2*(q1*q1+q2*q2));
    return eulerAngle;
}

void UTAlgorithm::Meter2LngLat(double &x, double &y) {
    x = x / METERS_PER_DEGREE;
    y = 180.0 / ALG_PI * (2 * atan(exp(y / R)) - ALG_PI / 2.0);
}

void UTAlgorithm::LngLat2Meter(double &x, double &y) {
    x = x * METERS_PER_DEGREE;
    y = log(tan((90.0 + y) * ALG_PI / 360.0)) * R;
}

END_SPACE_LANGE_VDR